/**
 * @file        motor_enco.c
 * @brief       电机编码器的读取驱动
 * @version     V1.2
 * @author      Jsir2022 (jsir2022@outlook.com)
 * @date        2024-02-29 13:10:53
 * LastEditors  Jsir2022
 * LastEditTime 2024-03-03 15:08:19
 * history		版本	日期		作者		说明
 * 				V1.0	2024-2-29	Jsir2022	添加了机械角度读取，但未能测试
 *              V1.1	2024-3-1	Jsir2022	测试完成，但读取不稳定不知为什么
 *              V1.2	2024-3-2	Jsir2022	新增计算实时速度
 * Copyright (c) 2024 by Jsir2022, All Rights Reserved.
 * @verbatim
 * ==============================================================================
 * 读取不稳定不知为什么，怀疑是线材接出来导致的
 * ==============================================================================
 * @endverbatim
 */
#include "motor_enco.h"
#include "motor_ctrl.h"

#include "spi.h"

/**
 * @brief   奇偶校验
 * @note
 * @param   {uint16_t} _Data 需要做校验的数据
 * @return  {*} 偶false，奇true
 */
bool Motor_Enco_getParity(uint16_t _Data)
{
    bool _Parity = false;
    while (_Data)
    {
        // 每执行一次，_EncoAll中1的数目就会减少1
        _Data &= _Data - 1;
        _Parity = !_Parity;
    }
    return _Parity;
}

//=================================用户修改======================================
/**
 * @brief   读取磁编的原始角度
 * @note
 * @return  {*}
 */
uint16_t Motor_Enco_readRawAngle(void)
{
    // MT6816角度读取
    uint8_t data_t[2];
    uint8_t data_r[2];
    uint16_t _EncoData;
    HAL_StatusTypeDef _ErrorCode = HAL_OK;

    data_t[0] = 0x83;
    data_t[1] = 0x84;

    uint8_t _Fault = 0;
    for (_Fault = 0; _Fault < 3; _Fault++)
    {
        Enco_CS_Down();
        HAL_SPI_Transmit(&hspi1, &data_t[0], 1, 10);
        _ErrorCode = HAL_SPI_Receive(&hspi1, &data_r[0], 1, 10);
        Enco_CS_Down();
        Enco_CS_Up();
        HAL_SPI_Transmit(&hspi1, &data_t[1], 1, 10);
        _ErrorCode = HAL_SPI_Receive(&hspi1, &data_r[1], 1, 10);
        Enco_CS_Up();

        if (_ErrorCode == HAL_TIMEOUT)
        {
            // 编码器未找到警告。如果未连接编码器是检测不出的，但连接错误可以
            Motor_Ctrl_errorHandler(__Enco_Not_Found);
            return NULL;
        }

        _EncoData = (data_r[0] << 8) | data_r[1];
#if Enable_Parity == false
        return _EncoData >> 2;
#else
        // 对数据进行奇偶校验
        if (Motor_Enco_getParity(_EncoData >> 1) == (_EncoData & 0x0001))
        {
            if ((_EncoData & 0x0002) == true)
            {
                // 弱磁警告
                Motor_Ctrl_errorHandler(__Enco_No_Mag);
                return NULL;
            }
            return _EncoData >> 2;
        }
#endif
    }
    // 奇偶校验多次失败警告
    Motor_Ctrl_errorHandler(__Enco_Parity_Fault);
    return NULL;
}

//===================================END========================================

/**
 * @brief   弧度制角度归一化
 * @note
 * @param   {float} _angle 输入的角度
 * @return  {*} 归一化后的角度
 */
float Motor_Enco_normAngle(float _angle)
{
    // 取余运算可以用于归一化
    _angle = fmod(_angle, 2 * M_PI);
    return _angle >= 0 ? _angle : (_angle + 2 * M_PI);
}

/**
 * @brief   读取机械角和电角度
 * @note
 * @param   {stMotorEnco_Para_type} *Enco_Para 编码器数据结构体指针
 * @param   {uint8_t} _MotorPairs 电机极对数
 * @return  {*} 返回电机电角度
 */
float Motor_Enco_readTheta(stMotorEnco_Para_type *Enco_Para, uint8_t _MotorPairs)
{
    uint16_t _rawAngle = Motor_Enco_readRawAngle();
    // 如果没发生错误就采用角度值
    if (_rawAngle != NULL)
    {
        Enco_Para->raw = (Enco_Para->dir == +1) ? Enco_Max - _rawAngle : _rawAngle;
    }

    // 磁编码器线性化，暂时没用
    int off_1 = Enco_Para->offset_lut[(Enco_Para->raw) >> 11];                                            // lookup table lower entry
    int off_2 = Enco_Para->offset_lut[((Enco_Para->raw >> 11) + 1) % 128];                                // lookup table higher entry
    int off_interp = off_1 + ((off_2 - off_1) * (Enco_Para->raw - ((Enco_Para->raw >> 11) << 11)) >> 11); // Interpolate between lookup table entries
    int count = Enco_Para->raw + off_interp;

    // 限制数值在编码器的单周期内
    if (count > Enco_Max)
        count -= Enco_Max;
    if (count < 0)
        count += Enco_Max;
    Enco_Para->count = count;

    int32_t delta_count = Enco_Para->count - Enco_Para->count_prev;
    Enco_Para->count_prev = Enco_Para->count;
    // 判断是否过了一个周期
    while (delta_count > +Enco_Max_Half)
        delta_count -= Enco_Max;
    while (delta_count < -Enco_Max_Half)
        delta_count += Enco_Max;
    Enco_Para->delta_count = delta_count;
    Enco_Para->speed = ((float)Enco_Para->delta_count / Enco_Max) * M_2PI;
    Enco_Para->acc_count += Enco_Para->delta_count;

    // 处理编码器数据获得机械角度，offs规定零点
    int32_t cali_theta = Enco_Para->count - Enco_Para->offset;

    if (cali_theta > Enco_Max)
        cali_theta -= Enco_Max;
    if (cali_theta < 0)
        cali_theta += Enco_Max;

    // 处理一下得到机械角和电角度
    Enco_Para->mec_angle = Motor_Enco_normAngle((float)cali_theta * M_2PI / Enco_Max);
    Enco_Para->elec_angle = Motor_Enco_normAngle(Enco_Para->mec_angle * _MotorPairs);
    return Enco_Para->elec_angle;
}

/**
 * @brief   计算电机转速
 * @note
 * @param   {stMotorEnco_Para_type} *Enco_Para 编码器数据结构体指针
 * @return  {*} 返回电机转速
 */
float Motor_Enco_caliSpeed(stMotorEnco_Para_type *Enco_Para)
{
    static float s_PrevAngle = 0;
    float _Speed;
    // 获得加速度
    _Speed = Enco_Para->mec_angle - s_PrevAngle;
    s_PrevAngle = Enco_Para->mec_angle;
    if (_Speed > M_PI)
    {
        _Speed -= M_2PI;
    }
    else if (_Speed < (-M_PI))
    {
        _Speed += M_2PI;
    }
    // 计算速度
    _Speed = _Speed / M_2PI / MotorCtrlTerm; // MotorCtrlTerm
    // Enco_Para->speed = _Speed;
    return _Speed;
}